package org.papervision3d.core.math;
/**
 * @author Tim Knip 
 */
class Quaternion 
{
	private var _matrix:Matrix3D;
	
	public static inline var EPSILON:Float = 0.000001;
	public static inline var DEGTORAD:Float = (Math.PI/180.0);
	public static inline var RADTODEG:Float = (180.0/Math.PI);
	
	/** */
	public var x:Float;
	
	/** */
	public var y:Float;
	
	/** */
	public var z:Float;
	
	/** */
	public var w:Float;
	
	/**
	 * inline varructor.
	 * 
	 * @param	x
	 * @param	y
	 * @param	z
	 * @param	w
	 * @return
	 */
	public function new( x:Float = 0, y:Float = 0, z:Float = 0, w:Float = 1 )
	{
		this.x = x;
		this.y = y;
		this.z = z;
		this.w = w;
		
		_matrix = Matrix3D.IDENTITY;
	}
	
	/**
	 * Clone.
	 * 
	 */
	public function clone():Quaternion
	{
		return  new Quaternion(this.x, this.y, this.z, this.w);
	}
	
	/**
	 * Multiply.
	 * 
	 * @param	a
	 * @param	b
	 */
	public function calculateMultiply( a:Quaternion, b:Quaternion ):Void
	{
		this.x = a.w*b.x + a.x*b.w + a.y*b.z - a.z*b.y;
		this.y = a.w*b.y - a.x*b.z + a.y*b.w + a.z*b.x;
		this.z = a.w*b.z + a.x*b.y - a.y*b.x + a.z*b.w;
		this.w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z;
	}
	
	/**
	 * Creates a Quaternion from a axis and a angle.
	 * 
	 * @param	x 	X-axis
	 * @param	y 	Y-axis
	 * @param	z 	Z-axis
	 * @param	angle	angle in radians.
	 * 
	 * @return
	 */
	public function setFromAxisAngle( x:Float, y:Float, z:Float, angle:Float ):Void
	{
		var sin:Float = Math.sin( angle / 2 );
		var cos:Float = Math.cos( angle / 2 );
		this.x = x * sin;
		this.y = y * sin;
		this.z = z * sin;
		this.w = cos;
		this.normalize();	
	}
	
	/**
	 * Sets this Quaternion from Euler angles.
	 * 
	 * @param	ax	X-angle in radians.
	 * @param	ay	Y-angle in radians.
	 * @param	az	Z-angle in radians.
	 */ 
	public function setFromEuler(ax:Float, ay:Float, az:Float, useDegrees:Bool=false):Void
	{
		if( useDegrees )
		{
			ax *= DEGTORAD;
			ay *= DEGTORAD;
			az *= DEGTORAD;
		}
		
		var fSinPitch       :Float = Math.sin( ax * 0.5 );
		var fCosPitch       :Float = Math.cos( ax * 0.5 );
		var fSinYaw         :Float = Math.sin( ay * 0.5 );
		var fCosYaw         :Float = Math.cos( ay * 0.5 );
		var fSinRoll        :Float = Math.sin( az * 0.5 );
		var fCosRoll        :Float = Math.cos( az * 0.5 );
		var fCosPitchCosYaw :Float = fCosPitch * fCosYaw;
		var fSinPitchSinYaw :Float = fSinPitch * fSinYaw;

		this.x = fSinRoll * fCosPitchCosYaw     - fCosRoll * fSinPitchSinYaw;
		this.y = fCosRoll * fSinPitch * fCosYaw + fSinRoll * fCosPitch * fSinYaw;
		this.z = fCosRoll * fCosPitch * fSinYaw - fSinRoll * fSinPitch * fCosYaw;
		this.w = fCosRoll * fCosPitchCosYaw     + fSinRoll * fSinPitchSinYaw;
	}
	
	/**
	 * Modulo.
	 * 
	 * @param	a
	 * @return
	 */
	public var modulo(get_modulo, null):Float;
	private function get_modulo():Float
	{
		return Math.sqrt(x*x + y*y + z*z + w*w);
	}
	
	/**
	 * Conjugate.
	 * 
	 * @param	a
	 * @return
	 */
	public static function conjugate( a:Quaternion ):Quaternion
	{
		var q:Quaternion = new Quaternion();
		q.x = -a.x;
		q.y = -a.y;
		q.z = -a.z;
		q.w = a.w;
		return q;
	}
	
	/**
	 * Creates a Quaternion from a axis and a angle.
	 * 
	 * @param	x 	X-axis
	 * @param	y 	Y-axis
	 * @param	z 	Z-axis
	 * @param	angle	angle in radians.
	 * 
	 * @return
	 */
	public static function createFromAxisAngle( x:Float, y:Float, z:Float, angle:Float ):Quaternion
	{
		var q:Quaternion = new Quaternion();

		q.setFromAxisAngle(x, y, z, angle);
		
		return q;
	}
	
	/**
	 * Creates a Quaternion from Euler angles.
	 * 
	 * @param	ax	X-angle in radians.
	 * @param	ay	Y-angle in radians.
	 * @param	az	Z-angle in radians.
	 * 
	 * @return
	 */
	public static function createFromEuler( ax:Float, ay:Float, az:Float, useDegrees:Bool = false ):Quaternion
	{
		if( useDegrees )
		{
			ax *= DEGTORAD;
			ay *= DEGTORAD;
			az *= DEGTORAD;
		}
		
		var fSinPitch       :Float = Math.sin( ax * 0.5 );
		var fCosPitch       :Float = Math.cos( ax * 0.5 );
		var fSinYaw         :Float = Math.sin( ay * 0.5 );
		var fCosYaw         :Float = Math.cos( ay * 0.5 );
		var fSinRoll        :Float = Math.sin( az * 0.5 );
		var fCosRoll        :Float = Math.cos( az * 0.5 );
		var fCosPitchCosYaw :Float = fCosPitch * fCosYaw;
		var fSinPitchSinYaw :Float = fSinPitch * fSinYaw;

		var q:Quaternion = new Quaternion();

		q.x = fSinRoll * fCosPitchCosYaw     - fCosRoll * fSinPitchSinYaw;
		q.y = fCosRoll * fSinPitch * fCosYaw + fSinRoll * fCosPitch * fSinYaw;
		q.z = fCosRoll * fCosPitch * fSinYaw - fSinRoll * fSinPitch * fCosYaw;
		q.w = fCosRoll * fCosPitchCosYaw     + fSinRoll * fSinPitchSinYaw;

		return q;
	}
			
	/**
	 * Creates a Quaternion from a matrix.
	 * 
	 * @param	matrix	a matrix. @see org.papervision3d.core.Matrix3D
	 * 
	 * @return	the created Quaternion
	 */
	public static function createFromMatrix( matrix:Matrix3D ):Quaternion
	{
		var quat:Quaternion = new Quaternion();
		
		var s:Float;
		var q:Array<Dynamic> = [];
		var i:Int, j:Int, k:Int;
		
		var tr:Float = matrix.n11 + matrix.n22 + matrix.n33;

		// check the diagonal
		if (tr > 0.0) 
		{
			s = Math.sqrt(tr + 1.0);
			quat.w = s / 2.0;
			s = 0.5 / s;
			
			quat.x = (matrix.n32 - matrix.n23) * s;
			quat.y = (matrix.n13 - matrix.n31) * s;
			quat.z = (matrix.n21 - matrix.n12) * s;
		} 
		else 
		{		
			// diagonal is negative
			var nxt:Array <Int> = [1, 2, 0];

			var m:Array<Array<Float>> = [
				[matrix.n11, matrix.n12, matrix.n13, matrix.n14],
				[matrix.n21, matrix.n22, matrix.n23, matrix.n24],
				[matrix.n31, matrix.n32, matrix.n33, matrix.n34]
			];
			
			i = 0;

			if (m[1][1] > m[0][0]) i = 1;
			if (m[2][2] > m[i][i]) i = 2;

			j = nxt[i];
			k = nxt[j];
			s = Math.sqrt((m[i][i] - (m[j][j] + m[k][k])) + 1.0);

			q[i] = s * 0.5;

			if (s != 0.0) s = 0.5 / s;

			q[3] = (m[k][j] - m[j][k]) * s;
			q[j] = (m[j][i] + m[i][j]) * s;
			q[k] = (m[k][i] + m[i][k]) * s;

			quat.x = q[0];
			quat.y = q[1];
			quat.z = q[2];
			quat.w = q[3];
		}
		return quat;
	}
	
	/**
	 * Creates a Quaternion from a orthonormal matrix.
	 * 
	 * @param	m	a orthonormal matrix. @see org.papervision3d.core.Matrix3D
	 * 
	 * @return  the created Quaternion
	 */
	public static function createFromOrthoMatrix( m:Matrix3D ):Quaternion
	{
		var q:Quaternion = new Quaternion();

		q.w = Math.sqrt( Math.max(0, 1 + m.n11 + m.n22 + m.n33) ) / 2;
		q.x = Math.sqrt( Math.max(0, 1 + m.n11 - m.n22 - m.n33) ) / 2;
		q.y = Math.sqrt( Math.max(0, 1 - m.n11 + m.n22 - m.n33) ) / 2;
		q.z = Math.sqrt( Math.max(0, 1 - m.n11 - m.n22 + m.n33) ) / 2;
		
		// recover signs
		q.x = m.n32 - m.n23 < 0 ? (q.x < 0 ? q.x : -q.x) : (q.x < 0 ? -q.x : q.x);
		q.y = m.n13 - m.n31 < 0 ? (q.y < 0 ? q.y : -q.y) : (q.y < 0 ? -q.y : q.y);
		q.z = m.n21 - m.n12 < 0 ? (q.z < 0 ? q.z : -q.z) : (q.z < 0 ? -q.z : q.z);

		return q;
	}
	
	/**
	 * Dot product.
	 * 
	 * @param	a
	 * @param	b
	 * 
	 * @return
	 */
	public static function dot( a:Quaternion, b:Quaternion ):Float
	{
		return (a.x * b.x) + (a.y * b.y) + (a.z * b.z) + (a.w * b.w);
	}
	
	/**
	 * Multiply.
	 * 
	 * @param	a
	 * @param	b
	 * @return
	 */
	public static function multiply( a:Quaternion, b:Quaternion ):Quaternion
	{
		var c:Quaternion = new Quaternion();
		c.x = a.w*b.x + a.x*b.w + a.y*b.z - a.z*b.y;
		c.y = a.w*b.y - a.x*b.z + a.y*b.w + a.z*b.x;
		c.z = a.w*b.z + a.x*b.y - a.y*b.x + a.z*b.w;
		c.w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z;
		return c;
	}
	
	/**
	 * Multiply by another Quaternion.
	 * 
	 * @param	b	The Quaternion to multiply by.
	 */
	public function mult( b:Quaternion ):Void
	{
		var aw:Float = this.w,
			ax:Float = this.x,
			ay:Float = this.y,
			az:Float = this.z;
			
		x = aw*b.x + ax*b.w + ay*b.z - az*b.y;
		y = aw*b.y - ax*b.z + ay*b.w + az*b.x;
		z = aw*b.z + ax*b.y - ay*b.x + az*b.w;
		w = aw*b.w - ax*b.x - ay*b.y - az*b.z;
	}
	
	public function toString():String{
		return "Quaternion: x:"+this.x+" y:"+this.y+" z:"+this.z+" w:"+this.w;
	}
	
	/**
	 * Normalize.
	 * 
	 * @param	a
	 * 
	 * @return
	 */
	public function normalize():Void
	{
		var len:Float = this.modulo;
		
		if( Math.abs(len) < EPSILON )
		{
			x = y = z = 0.0;
			w = 1.0;
		}
		else
		{
			var m:Float = 1 / len;
			x *= m;
			y *= m;
			z *= m;
			w *= m;
		}
	}

	/**
	 * SLERP (Spherical Linear intERPolation). @author Trevor Burton
	 * 
	 * @param	qa		start quaternion
	 * @param	qb		end quaternion
	 * @param	alpha	a value between 0 and 1
	 * 
	 * @return the interpolated quaternion.
	 */	
	public static function slerp( qa:Quaternion, qb:Quaternion, alpha:Float ):Quaternion
	{
		var angle:Float = qa.w * qb.w + qa.x * qb.x + qa.y * qb.y + qa.z * qb.z;

		 if (angle < 0.0)
		 {
				 qa.x *= -1.0;
				 qa.y *= -1.0;
				 qa.z *= -1.0;
				 qa.w *= -1.0;
				 angle *= -1.0;
		 }
 
		 var scale:Float;
		 var invscale:Float;
 
		 if ((angle + 1.0) > EPSILON) // Take the shortest path
		 {
				 if ((1.0 - angle) >= EPSILON)  // spherical interpolation
				 {
						 var theta:Float = Math.acos(angle);
						 var invsintheta:Float = 1.0 / Math.sin(theta);
						 scale = Math.sin(theta * (1.0-alpha)) * invsintheta;
						 invscale = Math.sin(theta * alpha) * invsintheta;
				 }
				 else // linear interploation
				 {
						 scale = 1.0 - alpha;
						 invscale = alpha;
				 }
		 }
		 else // long way to go...
		 {
			 qb.y = -qa.y;
			 qb.x = qa.x;
			 qb.w = -qa.w;
			 qb.z = qa.z;

			 scale = Math.sin(Math.PI * (0.5 - alpha));
			 invscale = Math.sin(Math.PI * alpha);
		 }
 
		return new Quaternion(  scale * qa.x + invscale * qb.x, 
								scale * qa.y + invscale * qb.y,
								scale * qa.z + invscale * qb.z,
								scale * qa.w + invscale * qb.w );
	}
	
	/**
	 * SLERP (Spherical Linear intERPolation).
	 * 
	 * @param	qa		start quaternion
	 * @param	qb		end quaternion
	 * @param	alpha	a value between 0 and 1
	 * 
	 * @return the interpolated quaternion.
	 */
	public static function slerpOld( qa:Quaternion, qb:Quaternion, alpha:Float ):Quaternion
	{
		var qm:Quaternion = new Quaternion();
		
		// Calculate angle between them.
		var cosHalfTheta:Float = qa.w * qb.w + qa.x * qb.x + qa.y * qb.y + qa.z * qb.z;

		// if qa=qb or qa=-qb then theta = 0 and we can return qa
		if(Math.abs(cosHalfTheta) >= 1.0)
		{
			qm.w = qa.w;
			qm.x = qa.x;
			qm.y = qa.y;
			qm.z = qa.z;
			return qm;
		}
		
		// Calculate temporary values.
		var halfTheta:Float = Math.acos(cosHalfTheta);
		var sinHalfTheta:Float = Math.sqrt(1.0 - cosHalfTheta*cosHalfTheta);
		
		// if theta = 180 degrees then result is not fully defined
		// we could rotate around any axis normal to qa or qb
		if(Math.abs(sinHalfTheta) < 0.001)
		{
			qm.w = (qa.w * 0.5 + qb.w * 0.5);
			qm.x = (qa.x * 0.5 + qb.x * 0.5);
			qm.y = (qa.y * 0.5 + qb.y * 0.5);
			qm.z = (qa.z * 0.5 + qb.z * 0.5);
			return qm;
		}
		
		var ratioA:Float = Math.sin((1 - alpha) * halfTheta) / sinHalfTheta;
		var ratioB:Float = Math.sin(alpha * halfTheta) / sinHalfTheta; 
		
		//calculate Quaternion.
		qm.w = (qa.w * ratioA + qb.w * ratioB);
		qm.x = (qa.x * ratioA + qb.x * ratioB);
		qm.y = (qa.y * ratioA + qb.y * ratioB);
		qm.z = (qa.z * ratioA + qb.z * ratioB);
		
		return qm;
	}
	
	public function toEuler():Number3D
	{
		var euler	:Number3D = new Number3D();
		var q1		:Quaternion = this;
		
		var test :Float = q1.x*q1.y + q1.z*q1.w;
		if (test > 0.499) { // singularity at north pole
			euler.x = 2 * Math.atan2(q1.x,q1.w);
			euler.y = Math.PI/2;
			euler.z = 0;
			return euler;
		}
		if (test < -0.499) { // singularity at south pole
			euler.x = -2 * Math.atan2(q1.x,q1.w);
			euler.y = - Math.PI/2;
			euler.z = 0;
			return euler;
		}
		
		var sqx	:Float = q1.x*q1.x;
		var sqy	:Float = q1.y*q1.y;
		var sqz	:Float = q1.z*q1.z;
		
		euler.x = Math.atan2(2*q1.y*q1.w-2*q1.x*q1.z , 1 - 2*sqy - 2*sqz);
		euler.y = Math.asin(2*test);
		euler.z = Math.atan2(2*q1.x*q1.w-2*q1.y*q1.z , 1 - 2*sqx - 2*sqz);
		
		return euler; 
	}
	
	/**
	 * Gets the matrix representation of this Quaternion.
	 * 
	 * @return matrix. @see org.papervision3d.core.Matrix3D
	 */
	public var matrix(get_matrix, null):Matrix3D;
	private function get_matrix():Matrix3D
	{
		var xx:Float = x * x;
		var xy:Float = x * y;
		var xz:Float = x * z;
		var xw:Float = x * w;
		var yy:Float = y * y;
		var yz:Float = y * z;
		var yw:Float = y * w;
		var zz:Float = z * z;
		var zw:Float = z * w;

		_matrix.n11 = 1 - 2 * ( yy + zz );
		_matrix.n12 =     2 * ( xy - zw );
		_matrix.n13 =     2 * ( xz + yw );
		
		_matrix.n21 =     2 * ( xy + zw );
		_matrix.n22 = 1 - 2 * ( xx + zz );
		_matrix.n23 =     2 * ( yz - xw );
		
		_matrix.n31 =     2 * ( xz - yw );
		_matrix.n32 =     2 * ( yz + xw );
		_matrix.n33 = 1 - 2 * ( xx + yy );
		
		return _matrix;
	}
	
	public static function sub(a:Quaternion, b:Quaternion):Quaternion
	{
		return new Quaternion(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w);	
	}
	
	public static function add(a:Quaternion, b:Quaternion):Quaternion
	{
		return new Quaternion(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w);	
	}
}

